/**
 * @file     ukf.h
 * @author   George Andrew Brindeiro
 * @date     06/10/2012
 *
 * @brief Unscented Kalman Filter class for the artoolkit_localization package
 *
 * This class implements UKF localization using ARToolkit markers, based on a
 * map of ARMarkers. The user must provide the node update rate, the filename
 * for the map and tf between the /camera and /odom frames.
 *
 * Contact: georgebrindeiro@lara.unb.br
 *
 * Revisions:
 * [06/10/2012] Created
 */

#ifndef UKF_H
#define UKF_H

#include <artoolkit_localization/ar_map.h>

// Standard C libraries
#include <cstdio>

// Eigen
#include <iostream>
#include <math.h>
#include <Eigen/Dense>
#include <vector>

#include <angles/angles.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <tf/transform_broadcaster.h>

// A macro to disallow the copy constructor and operator= functions
// This should be used in the private: declarations for a class
#define DISALLOW_COPY_AND_ASSIGN(TypeName) \
  TypeName(const TypeName&);               \
  void operator=(const TypeName&)

class UKF
{
    public:
        UKF(double dt = 0.1, std::string map_path = "cfg/lara.map")
			: map_(map_path)
        {
            dt_ = dt;

            Init();
        }

        ~UKF() {};

        // Public interface
        void localize(std::vector<double> controls, std::map< int, vector<double> > measurements);

        // Utility functions
		inline static double range(double dx, double dy){ return sqrt(dx*dx+dy*dy); }
		inline static double bearing(double dx, double dy, double theta){ return angles::normalize_angle(atan2(dy,dx)-theta); }

		geometry_msgs::PoseWithCovarianceStamped pose_msg();

        /* UKF Variables */
        Eigen::Vector3d state;
        Eigen::Matrix3d cov_state;

        Eigen::VectorXd aug_state;
        Eigen::MatrixXd cov_aug_state;

        Eigen::MatrixXd sigma_state;
        Eigen::MatrixXd sigma_controls;
        Eigen::MatrixXd sigma_measurements;
        Eigen::MatrixXd sigma_aug_state;

        Eigen::MatrixXd sigma_measurement;

        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    private:
        double dt_;			// UKF loop rate in seconds
		ARMap map_;			// UKF map

		void Init();

		void augment_state(std::vector<double> controls, std::map< int, vector<double> > measurements);
		void generate_sigma_points();
		void prediction(std::vector<double> controls);
		void update(std::map< int, vector<double> > measurements);
		void motion_model(Eigen::MatrixXd sigma_controls);
		void measurement_model(int measurement_id, Eigen::MatrixXd sigma_state, Eigen::MatrixXd sigma_measurements);
		Eigen::VectorXd weighted_mean(Eigen::MatrixXd X);
		Eigen::MatrixXd weighted_covariance(Eigen::MatrixXd X, Eigen::VectorXd x);
		Eigen::MatrixXd weighted_cross_covariance(Eigen::MatrixXd X, Eigen::VectorXd x, Eigen::MatrixXd Z, Eigen::VectorXd z);

		/* UKF Parameters */

		// Control Noise
		double alpha_vv;
		double alpha_vw;
		double alpha_wv;
		double alpha_ww;
		double std_v;
		double std_w;

		// Measurement Noise
		double std_rho;
		double std_phi;

		Eigen::Matrix2d cov_measurement;

		// Weights
	    /* UKF parameters */
		int n;
	    int s;
		double kappa;
		double wc;
		double we;

		/* Matrix sizes */
		int m;
		int a;

        DISALLOW_COPY_AND_ASSIGN(UKF);
};

#endif //UKF_H
